#include "anonymity.h"
#include <string.h>
#define rt_memcpy memcpy

uint32_t anonymity_check(uint8_t *data, uint32_t length, uint32_t *addcheck)
{
   uint32_t add = 0;
   uint32_t sum = 0; 
   for(int i = 0; i < length; i++) {
      sum += data[i];
      add += sum;
   }
   *addcheck = add;
   return sum;
}


int32_t anonymity_frame_pack(uint8_t *pack_buf, uint32_t pack_max_length,
                              anonymity_header_t *header, uint8_t *data,
                              uint32_t length) 
{
   if (!pack_buf || !pack_max_length || !length) {
      return -1;
   }
   
   if (header) {
      rt_memcpy(pack_buf, header, sizeof(anonymity_header_t));
   }
   
   if (pack_max_length < sizeof(anonymity_header_t) + length + 2) {
      return -2;
   }
   
   if (data) {
      rt_memcpy(pack_buf + sizeof(anonymity_header_t), data, length);
   }
   
   uint32_t sum = 0, add = 0;
   sum = anonymity_check(pack_buf, sizeof(anonymity_header_t) + length, &add);
   pack_buf[sizeof(anonymity_header_t) + length] = (uint8_t)(sum & 0xff);
   pack_buf[sizeof(anonymity_header_t) + length + 1] = (uint8_t)(add & 0xff);
   return sizeof(anonymity_header_t) + length + 2;
}

int32_t anonymity_frame_unpack(uint8_t *source_buff, uint32_t length,
                             anonymity_header_t *header, uint8_t *data, uint16_t max_length)
{
   if (!source_buff || !length || !data) {
      return -1;
   }
   // 判断数据长度是否正常
   if (length < sizeof(anonymity_header_t) + 2) {
      return -2;
   }
   // 判断数据开头是否为协议头字节
   if (source_buff[0] != 0xAB) {
      return -3;
   }
   
   anonymity_header_t *_header = (anonymity_header_t *)source_buff;
   
   // 有效数据长度大于实际数据长度
   if (_header->length > (length - (sizeof(anonymity_header_t) + 2))) {
      return -4;
   }
   
   if (header) {
      rt_memcpy(header, source_buff, sizeof(anonymity_header_t));
   }
   
   uint32_t sdd_chk = 0;
   uint8_t sum_chk = anonymity_check(source_buff, length - 2, &sdd_chk) & 0xff;
   if (sum_chk != source_buff[length - 2]) {
      return -5;
   }
   
   if ((sdd_chk & 0xff) != source_buff[length - 1]) {
      return -6;
   }
   
   if (_header->length > max_length) {
      rt_memcpy(data, source_buff + sizeof(anonymity_header_t), max_length);
   } else {
      rt_memcpy(data, source_buff + sizeof(anonymity_header_t), _header->length);
   }

   return header->length;
}


